package org.checkboo.math;

import java.nio.FloatBuffer;

import org.lwjgl.BufferUtils;

public class Matrix44f {
	
	private float v11,v12,v13,v14;
	private float v21,v22,v23,v24;
	private float v31,v32,v33,v34;
	private float v41,v42,v43,v44;
	
	private FloatBuffer fb;

	
	public Matrix44f(){
		zero();
	}
	
	public Matrix44f(Matrix44f matrix){
		set(matrix);
	}
	
	public Matrix44f(float v11, float v12, float v13, float v14, 
			float v21, float v22, float v23, float v24, 
			float v31, float v32, float v33, float v34, 
			float v41, float v42, float v43, float v44){
		this.v11 = v11;
		this.v12 = v12;
		this.v13 = v13;
		this.v14 = v14;
		this.v21 = v21;
		this.v22 = v22;
		this.v23 = v23;
		this.v24 = v24;
		this.v31 = v31;
		this.v32 = v32;
		this.v33 = v33;
		this.v34 = v34;
		this.v41 = v41;
		this.v42 = v42;
		this.v43 = v43;
		this.v44 = v44;
	}
	
	/**
	 * Adds supplied matrix to <code>this</code> matrix
	 * @param second matrix to add
	 * @return <code>this</code> matrix
	 */
	public Matrix44f add(Matrix44f second){
		this.v11 += second.v11;
		this.v12 += second.v12;
		this.v13 += second.v13;
		this.v14 += second.v14;
		this.v21 += second.v21;
		this.v22 += second.v22;
		this.v23 += second.v23;
		this.v24 += second.v24;
		this.v31 += second.v31;
		this.v32 += second.v32;
		this.v33 += second.v33;
		this.v34 += second.v34;
		this.v41 += second.v41;
		this.v42 += second.v42;
		this.v43 += second.v43;
		this.v44 += second.v44;
		return this;
	}
	
	/**
	 * Adds supplied matrix to <code>this</code> matrix and stores it in result
	 * @param second matrix to add
	 */
	public void addRes(Matrix44f second, Matrix44f result){
		result.set(this);
		result.add(second);
	}
	
	/**
	 * Subtracts supplied matrix from <code>this</code> matrix
	 * @param second matrix to subtract
	 * @return <code>this</code> matrix
	 */
	public Matrix44f subtract(Matrix44f second){
		this.v11 -= second.v11;
		this.v12 -= second.v12;
		this.v13 -= second.v13;
		this.v14 -= second.v14;
		this.v21 -= second.v21;
		this.v22 -= second.v22;
		this.v23 -= second.v23;
		this.v24 -= second.v24;
		this.v31 -= second.v31;
		this.v32 -= second.v32;
		this.v33 -= second.v33;
		this.v34 -= second.v34;
		this.v41 -= second.v41;
		this.v42 -= second.v42;
		this.v43 -= second.v43;
		this.v44 -= second.v44;
		return this;
	}
	
	/**
	 * Subtracts supplied matrix from <code>this</code> matrix and stores it in result
	 * @param second matrix to subtract
	 */
	public void subtractRes(Matrix44f second, Matrix44f result){
		result.set(this);
		result.subtract(second);
	}
	
	/**
	 * Multiplies <code>this</code> matrix with supplied matrix
	 * @param second matrix to multiply by
	 * @return <code>this</code> matrix
	 */
	public Matrix44f multiply(Matrix44f second){
		Matrix33f rotation = this.getRotation();
		Matrix33f rotationRes = new Matrix33f();
		
		rotation.multiplyRes(second.getRotation(),rotationRes);
		
		Vector3f transRes = rotation.multiply(second.getTranslation());
		transRes.add(this.getTranslation());
		
		this.rotate(rotationRes);
		
		this.translate(transRes);	
		
		/*Matrix44f tmp = new Matrix44f(this);
		
		this.v11 = tmp.v11*second.v11 + tmp.v12*second.v21 + tmp.v13*second.v31 + tmp.v14*second.v41;
		this.v12 = tmp.v11*second.v12 + tmp.v12*second.v22 + tmp.v13*second.v32 + tmp.v14*second.v42;
		this.v13 = tmp.v11*second.v13 + tmp.v12*second.v23 + tmp.v13*second.v33 + tmp.v14*second.v43;
		this.v14 = tmp.v11*second.v14 + tmp.v12*second.v24 + tmp.v13*second.v34 + tmp.v14*second.v44;
		
		this.v21 = tmp.v21*second.v11 + tmp.v22*second.v21 + tmp.v23*second.v31 + tmp.v24*second.v41;
		this.v22 = tmp.v21*second.v12 + tmp.v22*second.v22 + tmp.v23*second.v32 + tmp.v24*second.v42;
		this.v23 = tmp.v21*second.v13 + tmp.v22*second.v23 + tmp.v23*second.v33 + tmp.v24*second.v43;
		this.v24 = tmp.v21*second.v14 + tmp.v22*second.v24 + tmp.v23*second.v34 + tmp.v24*second.v44;
		
		this.v31 = tmp.v31*second.v11 + tmp.v32*second.v21 + tmp.v33*second.v31 + tmp.v34*second.v41;
		this.v32 = tmp.v31*second.v12 + tmp.v32*second.v22 + tmp.v33*second.v32 + tmp.v34*second.v42;
		this.v33 = tmp.v31*second.v13 + tmp.v32*second.v23 + tmp.v33*second.v33 + tmp.v34*second.v43;
		this.v34 = tmp.v31*second.v14 + tmp.v32*second.v24 + tmp.v33*second.v34 + tmp.v34*second.v44;
		
		this.v41 = tmp.v41*second.v11 + tmp.v42*second.v21 + tmp.v43*second.v31 + tmp.v44*second.v41;
		this.v42 = tmp.v41*second.v12 + tmp.v42*second.v22 + tmp.v43*second.v32 + tmp.v44*second.v42;
		this.v43 = tmp.v41*second.v13 + tmp.v42*second.v23 + tmp.v43*second.v33 + tmp.v44*second.v43;
		this.v44 = tmp.v41*second.v14 + tmp.v42*second.v24 + tmp.v43*second.v34 + tmp.v44*second.v44;
		*/
		return this;
	}
	
	/**
	 * Multiplies <code>this</code> matrix with supplied matrix and stores it in result
	 * @param second matrix to multiply by
	 */
	public void multiplyRes(Matrix44f second, Matrix44f result){
		result.set(this);
		result.multiply(second);
	}
	
	/**
	 * Transposes <code>this</code> matrix
	 * @return transposed matrix
	 */
	public Matrix44f transpose(){
		float[] tmp = new float[16];
		this.getValues(tmp, true);
		this.setValues(tmp, false);
		return this;
	}
	
	/**
	 * Transposes <code>this</code> matrix and stores it in result
	 * @param result matrix to store in
	 */
	public void transposeRes(Matrix44f result){
		result.set(this);
		result.transpose();
	}
	
	/**
	 * Performs rotation of <code>this</code> matrix around given axis by given angle
	 * @param angle to rotate by
	 * @param axis to rotate around
	 * @return <code>this</code> rotated matrix
	 */
	public Matrix44f rotate(float angle, Vector3f axis){
		Matrix33f tmp = new Matrix33f();
		tmp.fromAngleAxis(angle, axis);
		rotate(tmp);
		
		return this;
	}
	
	public Matrix44f rotate(Matrix33f matrix){
		this.v11 = matrix.getV11();
		this.v12 = matrix.getV12();
		this.v13 = matrix.getV13();
		this.v21 = matrix.getV21();
		this.v22 = matrix.getV22();
		this.v23 = matrix.getV23();
		this.v31 = matrix.getV31();
		this.v32 = matrix.getV32();
		this.v33 = matrix.getV33();
		
		return this;
	}
	
	/**
	 * Performs rotation of matrix around given axis by given angle and stores it in result
	 * @param angle to rotate by
	 * @param axis to rotate around
	 * @param result matrix to store in
	 */
	public void rotateRes(float angle, Vector3f axis, Matrix44f result){
		result.set(this);
		result.rotate(angle, axis);
	}
	
	/**
	 * Performs scaling of <code>this</code> matrix by given vector
	 * @param scale vector
	 * @return <code>this</code> scaled matrix
	 */
	public Matrix44f scale(Vector3f scale){
		Matrix33f tmp = new Matrix33f(v11, v12, v13, v21, v22, v23, v31, v32, v33);
		tmp.multiply(scale);
		rotate(tmp);
		return this;
	}
	
	/**
	 * Performs scaling of <code>this</code> matrix by given vector and stores it in result
	 * @param scale vector
	 * @param result matrix to store in
	 */
	public void scaleRes(Vector3f scale, Matrix44f result){
		result.set(this);
		result.scale(scale);
	}
	
	/**
	 * Performs translation of <code>this</code> matrix by given vector
	 * @param vector
	 * @return <code>this</code> translated matrix
	 */
	public Matrix44f translate(Vector3f vector){
		this.v14 = vector.getX();
		this.v24 = vector.getY();
		this.v34 = vector.getZ();
		return this;
	}
	
	/**
	 * Performs translation of <code>this</code> matrix by given vector and stores it in result
	 * @param vector
	 * @param result
	 */
	public void translateRes(Vector3f vector, Matrix44f result){
		result.set(this);
		result.translate(vector);
	}
	
	/**
	 * Computes determinant of <code>this</code> matrix
	 * @return value of determinant
	 */
	public float determinant(){
		float result = 0;
		Matrix33f temp = new Matrix33f(this.v22, this.v23, this.v24, this.v32, this.v33, this.v34, this.v42, this.v43, this.v44);
		result += this.v11 * temp.determinant();
		
		temp = new Matrix33f(this.v21, this.v23, this.v24, this.v31, this.v33, this.v34, this.v41, this.v43, this.v44);
		result -= this.v12 * temp.determinant();
		
		temp = new Matrix33f(this.v21, this.v22, this.v24, this.v31, this.v32, this.v34, this.v41, this.v42, this.v44);
		result += this.v13 * temp.determinant();
		
		temp = new Matrix33f(this.v21, this.v22, this.v23, this.v31, this.v32, this.v33, this.v41, this.v42, this.v43);
		result -= this.v14 * temp.determinant();
		
		return result;
	}
		
	/**
	 * Computes inverse matrix to <code>this</code> matrix
	 * @return <code>this</code> inversed matrix
	 */
	public Matrix44f inverse(){
		float det = this.determinant();
		if(det == 0){
			this.zero();
		}
		else{
			Matrix33f temp3;
			Matrix44f temp4 = new Matrix44f(this);
			temp3 = new Matrix33f(temp4.v22, temp4.v23, temp4.v24, temp4.v32, temp4.v33, temp4.v34, temp4.v42, temp4.v43, temp4.v44);
			this.v11 = temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v21, temp4.v23, temp4.v24, temp4.v31, temp4.v33, temp4.v34, temp4.v41, temp4.v43, temp4.v44);
			this.v12 = -temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v21, temp4.v22, temp4.v24, temp4.v31, temp4.v32, temp4.v34, temp4.v41, temp4.v42, temp4.v44);
			this.v13 = temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v21, temp4.v22, temp4.v23, temp4.v31, temp4.v32, temp4.v33, temp4.v41, temp4.v42, temp4.v43);
			this.v14 = -temp3.determinant()/det;
			
			
			temp3 = new Matrix33f(temp4.v12, temp4.v13, temp4.v14, temp4.v32, temp4.v33, temp4.v34, temp4.v42, temp4.v43, temp4.v44);
			this.v21 = -temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v13, temp4.v14, temp4.v31, temp4.v33, temp4.v34, temp4.v41, temp4.v43, temp4.v44);
			this.v22 = temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v12, temp4.v14, temp4.v31, temp4.v32, temp4.v34, temp4.v41, temp4.v42, temp4.v44);
			this.v23 = -temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v12, temp4.v13, temp4.v31, temp4.v32, temp4.v33, temp4.v41, temp4.v42, temp4.v43);
			this.v24 = temp3.determinant()/det;
			
			
			temp3 = new Matrix33f(temp4.v12, temp4.v13, temp4.v14, temp4.v22, temp4.v23, temp4.v24, temp4.v42, temp4.v43, temp4.v44);
			this.v31 = temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v13, temp4.v14, temp4.v21, temp4.v23, temp4.v24, temp4.v41, temp4.v43, temp4.v44);
			this.v32 = -temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v12, temp4.v14, temp4.v21, temp4.v22, temp4.v24, temp4.v41, temp4.v42, temp4.v44);
			this.v33 = temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v12, temp4.v13, temp4.v21, temp4.v22, temp4.v23, temp4.v41, temp4.v42, temp4.v43);
			this.v34 = -temp3.determinant()/det;
			
			
			temp3 = new Matrix33f(temp4.v12, temp4.v13, temp4.v14, temp4.v22, temp4.v23, temp4.v24, temp4.v32, temp4.v33, temp4.v34);
			this.v41 = -temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v13, temp4.v14, temp4.v21, temp4.v23, temp4.v24, temp4.v31, temp4.v33, temp4.v34);
			this.v42 = temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v12, temp4.v14, temp4.v21, temp4.v22, temp4.v24, temp4.v31, temp4.v32, temp4.v34);
			this.v43 = -temp3.determinant()/det;
			
			temp3 = new Matrix33f(temp4.v11, temp4.v12, temp4.v13, temp4.v21, temp4.v22, temp4.v23, temp4.v31, temp4.v32, temp4.v33);
			this.v44 = temp3.determinant()/det;
			
			this.transpose();
		}
		return this;
	}
	
	/**
	 * Computes inverse matrix to <code>this</code> matrix and stores it in result
	 * @param result
	 */
	public void inverseRes(Matrix44f result){
		result.set(this);
		result.inverse();
	}
	
	/**
	 * Computes negative of <code>this</code> matrix
	 */
	public Matrix44f negate(){
		this.v11 = -this.v11;
		this.v12 = -this.v12;
		this.v13 = -this.v13;
		this.v14 = -this.v14;
		this.v21 = -this.v21;
		this.v22 = -this.v22;
		this.v23 = -this.v23;
		this.v24 = -this.v24;
		this.v31 = -this.v31;
		this.v32 = -this.v32;
		this.v33 = -this.v33;
		this.v34 = -this.v34;
		this.v41 = -this.v41;
		this.v42 = -this.v42;
		this.v43 = -this.v43;
		this.v44 = -this.v44;
		
		return this;
	}
	
	/**
	 * Sets <code>this</code> matrix to identity matrix
	 */
	public Matrix44f identity(){
		this.v11 = 1;
		this.v12 = 0;
		this.v13 = 0;
		this.v14 = 0;
		this.v21 = 0;
		this.v22 = 1;
		this.v23 = 0;
		this.v24 = 0;
		this.v31 = 0;
		this.v32 = 0;
		this.v33 = 1;
		this.v34 = 0;
		this.v41 = 0;
		this.v42 = 0;
		this.v43 = 0;
		this.v44 = 1;
		
		return this;
	}
	
	/**
	 * Sets <code>this</code> matrix to zero matrix
	 */
	public void zero(){
		this.v11 = this.v12 = this.v13 = this.v14 = 0;
		this.v21 = this.v22 = this.v23 = this.v24 = 0;
		this.v31 = this.v32 = this.v33 = this.v34 = 0;
		this.v41 = this.v42 = this.v43 = this.v44 = 0;
	}
	
	/**
	 * Sets <code>this</code> matrix to given matrix
	 */
	public void set(Matrix44f matrix){
		this.v11 = matrix.v11;
		this.v12 = matrix.v12;
		this.v13 = matrix.v13;
		this.v14 = matrix.v14;
		this.v21 = matrix.v21;
		this.v22 = matrix.v22;
		this.v23 = matrix.v23;
		this.v24 = matrix.v24;
		this.v31 = matrix.v31;
		this.v32 = matrix.v32;
		this.v33 = matrix.v33;
		this.v34 = matrix.v34;
		this.v41 = matrix.v41;
		this.v42 = matrix.v42;
		this.v43 = matrix.v43;
		this.v44 = matrix.v44;
	}
	
	/**
	 * Gets values of <code>this</code> matrix ordered by rows or columns an stores them in target
	 * @param target array to store values in
	 * @param rows true-by rows   false-by columns
	 */
	public void getValues(float[] target, boolean rows){
		if(target.length == 16){
			if(rows){
				target[0] = this.v11;
				target[1] = this.v12;
				target[2] = this.v13;
				target[3] = this.v14;
				target[4] = this.v21;
				target[5] = this.v22;
				target[6] = this.v23;
				target[7] = this.v24;
				target[8] = this.v31;
				target[9] = this.v32;
				target[10] = this.v33;
				target[11] = this.v34;
				target[12] = this.v41;
				target[13] = this.v42;
				target[14] = this.v43;
				target[15] = this.v44;
			}
			else{
				target[0] = this.v11;
				target[1] = this.v21;
				target[2] = this.v31;
				target[3] = this.v41;
				target[4] = this.v12;
				target[5] = this.v22;
				target[6] = this.v32;
				target[7] = this.v42;
				target[8] = this.v13;
				target[9] = this.v23;
				target[10] = this.v33;
				target[11] = this.v43;
				target[12] = this.v14;
				target[13] = this.v24;
				target[14] = this.v34;
				target[15] = this.v44;
				}
			}
		else{
			System.out.println("Target array must have length of 16!!!");
		}
	}
	
	/**
	 * Sets values of <code>this</code> matrix to values from source
	 * @param source array to get values from
	 * @param rows true-by rows   false-by columns
	 */
	public void setValues(float[] source, boolean rows){
		if(source.length == 16){
			if(rows){
				this.v11 = source[0];
				this.v12 = source[1];
				this.v13 = source[2];
				this.v14 = source[3];
				this.v21 = source[4];
				this.v22 = source[5];
				this.v23 = source[6];
				this.v24 = source[7];
				this.v31 = source[8];
				this.v32 = source[9];
				this.v33 = source[10];
				this.v34 = source[11];
				this.v41 = source[12];
				this.v42 = source[13];
				this.v43 = source[14];
				this.v44 = source[15];
			}
			else{
				this.v11 = source[0];
				this.v21 = source[1];
				this.v31 = source[2];
				this.v41 = source[3];
				this.v12 = source[4];
				this.v22 = source[5];
				this.v32 = source[6];
				this.v42 = source[7];
				this.v13 = source[8];
				this.v23 = source[9];
				this.v33 = source[10];
				this.v43 = source[11];
				this.v14 = source[12];
				this.v24 = source[13];
				this.v34 = source[14];
				this.v44 = source[15];
				}
			}
		else{
			System.out.println("Target array must have length of 16!!!");
		}
	}
	
	public FloatBuffer toFloatBuffer(){
		if(null == this.fb)
			fb = BufferUtils.createFloatBuffer(16);
		fb.put(v11).put(v21).put(v31).put(v41);
		fb.put(v12).put(v22).put(v32).put(v42);
		fb.put(v13).put(v23).put(v33).put(v43);
		fb.put(v14).put(v24).put(v34).put(v44);
		fb.rewind();
		return fb;
	}
	
	public void fromFloatBuffer(FloatBuffer fb){
		fb.rewind();
		this.v11 = fb.get();
		this.v12 = fb.get();
		this.v13 = fb.get();
		this.v14 = fb.get();
		this.v21 = fb.get();
		this.v22 = fb.get();
		this.v23 = fb.get();
		this.v24 = fb.get();
		this.v31 = fb.get();
		this.v32 = fb.get();
		this.v33 = fb.get();
		this.v34 = fb.get();
		this.v41 = fb.get();
		this.v42 = fb.get();
		this.v43 = fb.get();
		this.v44 = fb.get();
	}
	
	public Matrix33f getRotation(){
		return new Matrix33f(v11, v12, v13, v21, v22, v23, v31, v32, v33);
	}
	
	public Vector3f getTranslation(){
		return new Vector3f(v14, v24, v34);
	}
	
	public float getV11() {
		return v11;
	}

	public void setV11(float v11) {
		this.v11 = v11;
	}

	public float getV12() {
		return v12;
	}

	public void setV12(float v12) {
		this.v12 = v12;
	}

	public float getV13() {
		return v13;
	}

	public void setV13(float v13) {
		this.v13 = v13;
	}

	public float getV14() {
		return v14;
	}

	public void setV14(float v14) {
		this.v14 = v14;
	}

	public float getV21() {
		return v21;
	}

	public void setV21(float v21) {
		this.v21 = v21;
	}

	public float getV22() {
		return v22;
	}

	public void setV22(float v22) {
		this.v22 = v22;
	}

	public float getV23() {
		return v23;
	}

	public void setV23(float v23) {
		this.v23 = v23;
	}

	public float getV24() {
		return v24;
	}

	public void setV24(float v24) {
		this.v24 = v24;
	}

	public float getV31() {
		return v31;
	}

	public void setV31(float v31) {
		this.v31 = v31;
	}

	public float getV32() {
		return v32;
	}

	public void setV32(float v32) {
		this.v32 = v32;
	}

	public float getV33() {
		return v33;
	}

	public void setV33(float v33) {
		this.v33 = v33;
	}

	public float getV34() {
		return v34;
	}

	public void setV34(float v34) {
		this.v34 = v34;
	}

	public float getV41() {
		return v41;
	}

	public void setV41(float v41) {
		this.v41 = v41;
	}

	public float getV42() {
		return v42;
	}

	public void setV42(float v42) {
		this.v42 = v42;
	}

	public float getV43() {
		return v43;
	}

	public void setV43(float v43) {
		this.v43 = v43;
	}

	public float getV44() {
		return v44;
	}

	public void setV44(float v44) {
		this.v44 = v44;
	}

	public boolean equals(Matrix44f second){
		if(this.v11 != second.v11)
			return false;
		if(this.v12 != second.v12)
			return false;
		if(this.v13 != second.v13)
			return false;
		if(this.v14 != second.v14)
			return false;
		if(this.v21 != second.v21)
			return false;
		if(this.v22 != second.v22)
			return false;
		if(this.v23 != second.v23)
			return false;
		if(this.v24 != second.v24)
			return false;
		if(this.v31 != second.v31)
			return false;
		if(this.v32 != second.v32)
			return false;
		if(this.v33 != second.v33)
			return false;
		if(this.v34 != second.v34)
			return false;
		if(this.v41 != second.v41)
			return false;
		if(this.v42 != second.v42)
			return false;
		if(this.v43 != second.v43)
			return false;
		if(this.v44 != second.v44)
			return false;
		
		return true;
	}
	
	public String toString(){
		String string = "[ "+this.v11+" ; "+this.v12+" ; "+this.v13+" ; "+this.v14+" ]\n" +
						"[ "+this.v21+" ; "+this.v22+" ; "+this.v23+" ; "+this.v24+" ]\n" +
						"[ "+this.v31+" ; "+this.v32+" ; "+this.v33+" ; "+this.v34+" ]\n" +
						"[ "+this.v41+" ; "+this.v42+" ; "+this.v43+" ; "+this.v44+" ]\n";
		return string;
	}
}
